
#ifndef OP_FIX2POSE_
#define OP_FIX2POSE_

#include <string>
#include <vector>
#include <algorithm>
#include <proj.h>
#include "rclcpp/rclcpp.hpp"
#include <sensor_msgs/msg/nav_sat_fix.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>

class Fix2PoseInterface : public rclcpp::Node
{
public:
   explicit Fix2PoseInterface(const rclcpp::NodeOptions & node_options);
   virtual ~Fix2PoseInterface();
   std::string tf_output_frame_;
  
private:

  rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr sub_gnss_fix;
  rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pup_pose;
  rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pup_pose_with_cov;


  void GnssCallBack(const sensor_msgs::msg::NavSatFix::SharedPtr msg);

};


/// \file MappingHelpers.h
/// \brief Helper functions for mapping operation such as (load and initialize vector maps , convert map from one format to another, .. )
/// \author Hatem Darweesh
/// \date Jul 2, 2016




#include "planning/RoadNetwork.h"
#include "utilities/data_rw.h"
#include <math.h>
#include <tinyxml.h>

namespace op {
	namespace mapping {


class MappingHelpers {
public:
	MappingHelpers();
	virtual ~MappingHelpers();

	static void llaToxyz_proj(const std::string& proj_str, const WayPoint& origin, const double& lat,
			const double& lon, const double& alt, double& x_out, double& y_out, double& z_out);

	static void xyzTolla_proj(const std::string& proj_str, const WayPoint& origin, const double& x_in,
			const double& y_in, const double& z_in, double& lat, double& lon, double& alt);
};

} /* namespace mapping */
} /* namespace op */

#endif /* MAPPINGHELPERS_H_ */


